#include <ros/ros.h>
#include <std_msgs/String.h>
#include <serial/serial.h>
#include <std_msgs/Int32.h>

int motor_dis = 0;
int mag_mark = 0;
int cam_mark = 0;
int Baudrate = 57600;
std::string usbid = "/dev/ttyUSB0";
std::vector<double> dis_wave(6);
serial::Serial serialPort;

void send_msg(){
    // 发送
    // 构造报文
    std::vector<uint8_t> message;
    message.push_back(0xEF);

    message.push_back(motor_dis & 0xFF);
    message.push_back(motor_dis>>8 & 0xFF);

    message.push_back(mag_mark & 0xFF);
    message.push_back(cam_mark & 0xFF);

    try {
        serialPort.write(message);
    } catch (const serial::PortNotOpenedException& e) {
        printf("Failed to send message: Port not opened\n");
    } catch (const std::exception& e) {
        printf("Failed to send message: %s\n", e.what());
    }
}

void MotorCallback(const std_msgs::Int32::ConstPtr& msg) {
    motor_dis = msg->data;
    send_msg();
}

void MagCallback(const std_msgs::Int32::ConstPtr& msg) {
    if (msg->data == 1) mag_mark = 0;
    if (msg->data == 2) mag_mark = 1;
    send_msg();
}

void CamCallback(const std_msgs::Int32::ConstPtr& msg) {
    cam_mark = msg->data;
    send_msg();
}

int main(int argc, char** argv) {
    // 初始化 ROS 节点
    ros::init(argc, argv, "serial_reader_node");
    ros::NodeHandle nh;
    if (nh.getParam("/arduino_uart_node/usbid", usbid)) {
        printf("usbid: %s\n", usbid.c_str());
    }
    if (nh.getParam("/arduino_uart_node/Baudrate", Baudrate)) {
        printf("Baudrate: %i\n", Baudrate);
    }
    // 发布串口数据的主题
    ros::Subscriber sub_motor = nh.subscribe("/arduino/motor", 10, MotorCallback);
    ros::Subscriber sub_mag   = nh.subscribe("/arduino/mag"  , 10, MagCallback  );
    ros::Subscriber sub_cam   = nh.subscribe("/arduino/cam"  , 10, CamCallback  );

    try {
        // 配置串口
        serialPort.setPort(usbid); // 指定串口设备
        serialPort.setBaudrate((uint32_t)Baudrate); // 设置波特率
        serial::Timeout to = serial::Timeout::simpleTimeout(1000); // 设置超时
        serialPort.setTimeout(to);
        serialPort.open(); // 打开串口
    } catch (serial::IOException& e) {
        printf("Unable to open port: %s\n", e.what());
        return -1;
    }


    if (serialPort.isOpen()) {
        printf("Serial port initialized\n");
    } else {
        printf("Failed to initialize serial port\n");
        return -1;
    }

    ros::Rate loop_rate(50); // 循环频率(50 Hz)

    long long number = 0;
    while (ros::ok()) {

        ros::spinOnce();
    }

    serialPort.close(); // 关闭串口
    return 0;
}

